/*
 * File - ros_pointcloud_publisher.cpp
 *
 * This file is part of the Inuitive SDK
 *
 * Copyright (C) 2014-2020 Inuitive All Rights Reserved
 *
 */

/**
 * \cond INTERNAL
 */

#include "config.h"

#include <fstream>
#include <iostream>

#if POINTCLOUD_DUMP_FRAME
#include <experimental/filesystem>
#endif

#include <nodelet/nodelet.h>
#include <ros/rate.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/distortion_models.h>

#include <opencv2/opencv.hpp>

#ifdef USE_PC_FILTER
#include "pc_filter.h"
#endif
/**
 * \endcond
 */

#include "ros_pointcloud_publisher.h"

#include "inudev_ros_nodelet.h"



#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>


/*
#include <pcl/visualization/cloud_viewer.h>
void simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
    //创建3D窗口并添加点云
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    viewer->spin();
}
*/

/**
 * \file ros_pointcloud_publisher.cpp
 *
 * \brief CRosPointcloudPublisher
 */

/**
 * \cond INTERNAL
 */

static const char rcsid[]="$Id: 005b327968f410296d2b95ce626635748202250d $";

/**
 * \endcond
 */

namespace __INUROS__NAMESPACE__
{
   // std::vector<const char *> CRosPointcloudPublisher::fieldNames = { "x", "y", "z", "b", "g", "r", "a" };
    std::vector<const char *> CRosPointcloudPublisher::fieldNames = { "x", "y", "z" };

    CRosPointcloudPublisher::CRosPointcloudPublisher(ros::NodeHandle& _node,  std::shared_ptr<image_transport::ImageTransport> _image_transport, CRosSensor* _rosSensor, std::string iTopicName)
        : CRosPublisher(_node, _image_transport, "pointcloud", _rosSensor)
        , publisher(node.advertise<sensor_msgs::PointCloud2>(iTopicName, 1))
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());
    }


    void CRosPointcloudPublisher::FrameCallback(std::shared_ptr<InuDev::CPointCloudStream> iStream, std::shared_ptr<const InuDev::CPointCloudFrame> iFrame, InuDev::CInuError iError)
    {
        NODELET_DEBUG_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        if (!CheckFrame(iFrame, iError))
        {
            return;
        }

        InuDev::CPointCloudFrame::EFormat format = (InuDev::CPointCloudFrame::EFormat)iFrame->GetFormat();

        if (format != InuDev::CPointCloudFrame::EFormat::e3DPoints && format != InuDev::CPointCloudFrame::EFormat::e3DPointsRGB && format != InuDev::CPointCloudFrame::EFormat::e3DShortPoints) 
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Frame's format (" << format << ") is not ePointCloud");
            return;
        }

        int numPoints = iFrame->GetNumOfPoints();

        //fillCameraInfoFromVideo(iFrame->FrameIndex, width, height, true);

        sensor_msgs::PointCloud2Ptr cloudMsg(new sensor_msgs::PointCloud2);

        cloudMsg->header.stamp.sec  = iFrame->Timestamp / 1000000000ll;
        cloudMsg->header.stamp.nsec = iFrame->Timestamp % 1000000000ll;

        ros::param::param<std::string>("Device_ID", mPointCloudFrameID, "");
        cloudMsg->header.frame_id = "camera_link"+mPointCloudFrameID;

        // "A PointCloud2 can be organized or unorganized. Unorganized means that height == 1 (and
        // therefore width == number of points, since height * width must equal number of points).
        // Organized means something like height == 240, width == 320, and that's usual for point
        // clouds that come from a 3D camera. The useful thing about this is that there is a
        // one-to-one mapping between a pixel and a point, which allows more efficient algorithms."
        //
        // https://answers.ros.org/question/234455/pointcloud2-and-pointfield/

        cloudMsg->height = 1; // unorganized
        cloudMsg->width = numPoints;

        cloudMsg->fields.resize(fieldNames.size());

        if ((format == InuDev::CPointCloudFrame::EFormat::e3DPoints) || (format == InuDev::CPointCloudFrame::EFormat::e3DShortPoints && mConvertPointCloudShortToFloat)) 
        {
            for (std::vector<char *>::size_type fldIndex=0; fldIndex < 3; fldIndex++)
            {
                auto &fld(cloudMsg->fields[fldIndex]);

                fld.name     = fieldNames[fldIndex];
                fld.offset   = fldIndex*sizeof(float);
                fld.datatype = sensor_msgs::PointField::FLOAT32;
                fld.count    = 1;
            }
            cloudMsg->point_step = 3 * sizeof(float);
        }
        else if (format == InuDev::CPointCloudFrame::EFormat::e3DShortPoints && mConvertPointCloudShortToFloat == false)
        {
            for (std::vector<char *>::size_type fldIndex=0; fldIndex < 3; fldIndex++)
            {
                auto &fld(cloudMsg->fields[fldIndex]);

                fld.name     = fieldNames[fldIndex];
                fld.offset   = fldIndex*sizeof(short);
                fld.datatype = sensor_msgs::PointField::INT16;
                fld.count    = 1;
            }
            cloudMsg->point_step = 3 * sizeof(short);
        }
        else if (format == InuDev::CPointCloudFrame::EFormat::e3DPointsRGB) 
        {
                for (std::vector<char *>::size_type fldIndex=3; fldIndex < fieldNames.size(); fldIndex++)  // 3, 4, 5, 6
                {
                    auto &fld(cloudMsg->fields[fldIndex]);

                    fld.name     = fieldNames[fldIndex];
                    fld.offset   = fldIndex + 9;                                                           // 12 13 14 15
                    fld.datatype = sensor_msgs::PointField::UINT8;
                    fld.count    = 1;
                }
                cloudMsg->point_step = 3 * sizeof(float) + 4;        
        }
        else
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " unknown format: " << format);
            return;
        }

        cloudMsg->row_step = cloudMsg->point_step;

        // "A PointCloud2 can be dense or non-dense. Dense means that there are no invalid points, 
        // non-dense means the opposite. An invalid point is one that has all x/y/z values set to
        // std::numeric_limits<float>::quiet_NaN()."

        //NODELET_WARN_STREAM("row_step: " << cloudMsg->row_step);

        cloudMsg->is_dense = true; // all data points are valid
        cloudMsg->is_bigendian = false;
        cloudMsg->data.resize(cloudMsg->point_step * numPoints);
        if (format == InuDev::CPointCloudFrame::EFormat::e3DPoints || format == InuDev::CPointCloudFrame::EFormat::e3DShortPoints && mConvertPointCloudShortToFloat == false) 
        {
            std::memcpy(cloudMsg->data.data(), iFrame->GetData(), cloudMsg->data.size());
        } 
        else if (format == InuDev::CPointCloudFrame::EFormat::e3DShortPoints && mConvertPointCloudShortToFloat == true)
        {
            float* floatArr = (float*)cloudMsg->data.data();
            short* shortArr = (short*)iFrame->GetData();
            for(int i=0;i<numPoints;i+=3)
            {
                floatArr[i] = shortArr[i];
                floatArr[i+1] = shortArr[i+1];
                floatArr[i+2] = shortArr[i+2]; 
            }
        }
        else if(format == InuDev::CPointCloudFrame::EFormat::e3DPointsRGB)
        {
            std::memcpy(cloudMsg->data.data(), iFrame->Get3DRGBData(), cloudMsg->data.size());
        }
#ifdef USE_PC_FILTER
        if (CInuDevRosNodelet::config.PCL_filter)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

            pcl::fromROSMsg(*cloudMsg, *cloud_in);
            pc_remove_outlier(cloud_in,cloud_out);
            sensor_msgs::PointCloud2Ptr msg(new sensor_msgs::PointCloud2);
            pcl::toROSMsg(*cloud_out, *msg);
            publisher.publish(msg);
        }
        else
#endif
            publisher.publish(cloudMsg);


#if POINTCLOUD_DUMP_FRAME

        if (CInuDevRosNodelet::mLogPointcloud)
        {
            // print using 'od --format=f4 --width=12 /tmp/aaaaa.bin'

            std::fstream outFile;

            std::experimental::filesystem::path fileNameTemp(POINTCLOUD_DUMP_FILENAME ".bin.tmp");
            outFile.open(std::string(fileNameTemp), std::ios::out);

            if (format != InuDev::CPointCloudFrame::EFormat::e3DPointsRGB) {
                outFile.write(reinterpret_cast<const char *>(iFrame->GetData()), cloudMsg->data.size());
            } else {
                outFile.write(reinterpret_cast<const char *>(iFrame->Get3DRGBData()), cloudMsg->data.size());
            }
            outFile.close();

            std::experimental::filesystem::path fileName(POINTCLOUD_DUMP_FILENAME "_");
            fileName += std::to_string(iFrame->FrameIndex);
            fileName += ".bin";

            NODELET_INFO_STREAM("filename=" << std::string(fileName));

            std::experimental::filesystem::rename(fileNameTemp, fileName);
        }

#endif

        if (CInuDevRosNodelet::config.pointcloud_verbose)
        {
            static auto t1 = std::chrono::steady_clock::now();
            static int sidx = 0;
            sidx++;
            if (sidx >= 10)
            {
                double dr_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - t1).count();

                t1 = std::chrono::steady_clock::now();

                NODELET_WARN_STREAM("PC Time: " << dr_ms << ", FPS: " << int(1000 / (dr_ms/10)));
                sidx = 0;
            }

        }
    }

    InuDev::CInuError CRosPointcloudPublisher::RegisterCallback()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        InuDev::CPointCloudStream::CallbackFunction callback = std::bind(&CRosPointcloudPublisher::FrameCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);

        return std::static_pointer_cast<InuDev::CPointCloudStream>(stream)->Register(callback);
    }

    InuDev::CInuError CRosPointcloudPublisher::UnregisterCallback()
    {
        return std::static_pointer_cast<InuDev::CPointCloudStream>(stream)->Register(nullptr);
    }

    int CRosPointcloudPublisher::GetNumSubscribers()
    {
        publisherPrev = publisherCurr;
        publisherCurr = publisher.getNumSubscribers();

        return publisherCurr;
    }

    InuDev::CInuError CRosPointcloudPublisher::InitStream()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        // Setup as IR
        // InuDev::CInuError err = std::static_pointer_cast<InuDev::CPointCloudStream>(stream)->Init(InuDev::CPointCloudStream::eRGBRegistation, 4);

        InuDev::CInuError err(0);

        if (mOutputFormat == 0)
        {
            err = std::static_pointer_cast<InuDev::CPointCloudStream>(stream)->Init(InuDev::CPointCloudStream::ERegistationType::eNoneRegistation);
        }
        else 
        {
            if (mParamsChannelID == 0){
                err = std::static_pointer_cast<InuDev::CPointCloudStream>(stream)->Init(InuDev::CPointCloudStream::ERegistationType::eRGBRegistation);   
            } 
            else 
            {
                err = std::static_pointer_cast<InuDev::CPointCloudStream>(stream)->Init(InuDev::CPointCloudStream::ERegistationType::eRGBRegistation, mParamsChannelID);
            }
        }


        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        return InuDev::EErrorCode::eOK;
    }

    InuDev::CInuError CRosPointcloudPublisher::StartStream()
    {

        // Load default params from server
        ros::param::param<int>("PointCloudFormat", mOutputFormat, 0);
        ros::param::param<int>("PointCloudRegistrationChannel", mParamsChannelID, -1);
        ros::param::param<bool>("ConvertPointCloudShortToFloat", mConvertPointCloudShortToFloat, true);

        NODELET_WARN_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << "RGB Channel: " << CInuDevRosNodelet::config.PCL_RGB_channel_ID << " PCL Format: " << CInuDevRosNodelet::config.PCL_output_format << "  mConvertPointCloudShortToFloat: " << mConvertPointCloudShortToFloat << " DDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDd");

        // check for user dynamic changes, 0, 4, 9 
        if(CInuDevRosNodelet::config.PCL_RGB_channel_ID != -1)
        {
            mParamsChannelID = CInuDevRosNodelet::config.PCL_RGB_channel_ID;
        }

        // 0, 1 
        if(CInuDevRosNodelet::config.PCL_output_format != 0)
        {
            mOutputFormat = CInuDevRosNodelet::config.PCL_output_format;
        }

        NODELET_WARN_STREAM("PC: format " << mOutputFormat << ",Channel: " << mParamsChannelID );

        stream = std::static_pointer_cast<InuDev::CBaseStream>(sensor->getSensor()->CreatePointCloudStream());

        NODELET_ERROR_STREAM_COND(!stream, __INUROS_FUNCTION_NAME__ << ": failed creating " << getName());

        InuDev::EErrorCode err = CRosPublisher::StartStream();

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        return InuDev::EErrorCode::eOK;
    }

}
